#ifndef COSTMAP_2D_TESTING_HELPER_H
#define COSTMAP_2D_TESTING_HELPER_H

#include<costmap_2d/cost_values.h>
#include<costmap_2d/costmap_2d.h>
#include <costmap_2d/static_layer.h>
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/inflation_layer.h>

#include <sensor_msgs/point_cloud2_iterator.h>

const double MAX_Z(1.0);

char printableCost(unsigned char cost)
{
    switch (cost)
    {
        case costmap_2d::NO_INFORMATION:
            return '?';
        case costmap_2d::LETHAL_OBSTACLE:
            return 'L';
        case costmap_2d::INSCRIBED_INFLATED_OBSTACLE:
            return 'I';
        case costmap_2d::FREE_SPACE:
            return '.';
        default:
            return '0' + (unsigned char) (10 * cost / 255);
    }
}

void printMap(costmap_2d::Costmap2D &costmap)
{
    printf("map:\n");
    for (int i = 0; i < costmap.getSizeInCellsY(); i++)
    {
        for (int j = 0; j < costmap.getSizeInCellsX(); j++)
        {
            printf("%4d", int(costmap.getCost(j, i)));
        }
        printf("\n\n");
    }
}

unsigned int countValues(costmap_2d::Costmap2D &costmap, unsigned char value, bool equal = true)
{
    unsigned int count = 0;
    for (int i = 0; i < costmap.getSizeInCellsY(); i++)
    {
        for (int j = 0; j < costmap.getSizeInCellsX(); j++)
        {
            unsigned char c = costmap.getCost(j, i);
            if ((equal && c == value) || (!equal && c != value))
            {
                count += 1;
            }
        }
    }
    return count;
}

void addStaticLayer(costmap_2d::LayeredCostmap &layers, tf2_ros::Buffer &tf)
{
    costmap_2d::StaticLayer *slayer = new costmap_2d::StaticLayer();
    layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(slayer));
    slayer->initialize(&layers, "static", &tf);
}

costmap_2d::ObstacleLayer *addObstacleLayer(costmap_2d::LayeredCostmap &layers, tf2_ros::Buffer &tf)
{
    costmap_2d::ObstacleLayer *olayer = new costmap_2d::ObstacleLayer();
    olayer->initialize(&layers, "obstacles", &tf);
    layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(olayer));
    return olayer;
}

void addObservation(costmap_2d::ObstacleLayer *olayer, double x, double y, double z = 0.0,
                    double ox = 0.0, double oy = 0.0, double oz = MAX_Z)
{
    sensor_msgs::PointCloud2 cloud;
    sensor_msgs::PointCloud2Modifier modifier(cloud);
    modifier.setPointCloud2FieldsByString(1, "xyz");
    modifier.resize(1);
    sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
    sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
    sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
    *iter_x = x;
    *iter_y = y;
    *iter_z = z;

    geometry_msgs::Point p;
    p.x = ox;
    p.y = oy;
    p.z = oz;

    costmap_2d::Observation obs(p, cloud, 100.0, 100.0);  // obstacle range = raytrace range = 100.0
    olayer->addStaticObservation(obs, true, true);
}

costmap_2d::InflationLayer *addInflationLayer(costmap_2d::LayeredCostmap &layers, tf2_ros::Buffer &tf)
{
    costmap_2d::InflationLayer *ilayer = new costmap_2d::InflationLayer();
    ilayer->initialize(&layers, "inflation", &tf);
    boost::shared_ptr<costmap_2d::Layer> ipointer(ilayer);
    layers.addPlugin(ipointer);
    return ilayer;
}

#endif  // COSTMAP_2D_TESTING_HELPER_H
